
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.templates.commands.*;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ShooterSubsystem;

public class OI {
    public static final int kLogitechDriverPort = 1;
    public static final int kLogitechCoDriverPort = 2;
    public static final int kLogitechCalibrationPort = 3;

    
    private LogitechController m_driverGamePad;
    private LogitechControllerButton m_setDriverForwardButton;
    private LogitechControllerButton m_setDriverReverseButton;
    private LogitechControllerButton m_setDriverLeftButton;
    private LogitechControllerButton m_setDriverRightButton;
    private LogitechControllerButton m_setDriverHighClearanceButton;
    private LogitechControllerButton m_setDriverHighTractionButton;
    private LogitechControllerButton m_setDriverBackButton;
    private LogitechControllerButton m_setDriverStartButton;
    private LogitechControllerButton m_setDriverRightTriggerButton;
    
    private LogitechController m_coDriverGamePad;
    private LogitechControllerButton m_setCoDriverForwardButton;
    private LogitechControllerButton m_setCoDriverReverseButton;
    private LogitechControllerButton m_setCoDriverLeftButton;
    private LogitechControllerButton m_setCoDriverRightButton;
    private LogitechControllerButton m_setCoDriverShoulderButton;
    private LogitechControllerButton m_setCoDriverBackButton;
    private LogitechControllerButton m_setCoDriverStartButton;
    private LogitechControllerButton m_setCoDriverRightTriggerButton;
    private LogitechControllerButton m_setCoDriverLeftTriggerButton;
    private LogitechControllerButton m_setCoDriverLeftShoulderButton;
    private LogitechControllerButton m_setCoDriverGamepadLeftButton;
    private LogitechControllerButton m_setCoDriverGamepadRightButton;
    private LogitechControllerButton m_setCoDriverLeftTopButton;

    private LogitechController m_CalibrationGamePad;
    private LogitechControllerButton m_setCalibratorLeftShoulderButton;
    private LogitechControllerButton m_setCalibratorLeftTriggerButton;
    private LogitechControllerButton m_setCalibratorRightShoulderButton;
    private LogitechControllerButton m_setCalibratorRightTriggerButton;

    
    // Process operator interface input here.
            
    public OI() {
       //
       //
       // Driver Controller - Moving the robot
       //
       //
       m_driverGamePad = new LogitechController(kLogitechDriverPort);
       // Drive Direction
       m_setDriverForwardButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonY.value);     
       m_setDriverReverseButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonA.value);
       m_setDriverLeftButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonX.value);
       m_setDriverRightButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonB.value);

       m_setDriverForwardButton.whenPressed(new newDriveDirection(newDriveDirection.Direction.kFore));
       m_setDriverReverseButton.whenPressed(new newDriveDirection(newDriveDirection.Direction.kRear));
       m_setDriverLeftButton.whenPressed(new newDriveDirection(newDriveDirection.Direction.kLeft));
       m_setDriverRightButton.whenPressed(new newDriveDirection(newDriveDirection.Direction.kRight));
       
       // High Clearance       
       m_setDriverHighClearanceButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonRightB.value);       
       m_setDriverHighClearanceButton.whenPressed(new HighClearance(true));
//       m_setDriverHighClearanceButton.whenReleased(new HighClearance(false));

       // High Traction
       m_setDriverHighTractionButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonRightTrigger.value);              
       m_setDriverHighTractionButton.whenPressed(new HighTraction(true));
//       m_setDriverHighTractionButton.whenReleased(new HighTraction(false));

       // Joystick Recenter
       m_setDriverBackButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonBack.value);
       m_setDriverBackButton.whenPressed(new Recenter());      
       

       // Open Buttons
       //m_setDriverStartButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonStart.value);
       //m_setDriverRightTriggerButton = new LogitechControllerButton(m_driverGamePad, LogitechController.ButtonType.kButtonRightTop.value);

       //
       //
       // CoDriver Controller - Arm & Scoring Mechanisms
       //
       //
       m_coDriverGamePad = new LogitechController(kLogitechCoDriverPort);
       // Lifter Balance
       m_setCoDriverForwardButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonY.value);     
       m_setCoDriverReverseButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonA.value);
       m_setCoDriverLeftButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonX.value);
       m_setCoDriverRightButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonB.value);
       
       m_setCoDriverForwardButton.whenPressed(new LifterBalance(CommandBase.leftForeLifterSubsystem, CommandBase.rightForeLifterSubsystem));
       m_setCoDriverReverseButton.whenPressed(new LifterBalance(CommandBase.leftRearLifterSubsystem, CommandBase.rightRearLifterSubsystem));
       m_setCoDriverLeftButton.whenPressed(new LifterBalance(CommandBase.leftForeLifterSubsystem, CommandBase.leftRearLifterSubsystem));
       m_setCoDriverRightButton.whenPressed(new LifterBalance(CommandBase.rightForeLifterSubsystem, CommandBase.rightRearLifterSubsystem));

       // Arm Drive
       m_setCoDriverLeftTriggerButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonLeftTrigger.value);
       m_setCoDriverLeftShoulderButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonLeftB.value);
       m_setCoDriverBackButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonBack.value);
       
       m_setCoDriverLeftTriggerButton.whenPressed(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kLowered));
       m_setCoDriverLeftShoulderButton.whenPressed(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kRaised));
       m_setCoDriverBackButton.whenPressed(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kIntermediate));
               
       m_setCoDriverLeftTopButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonLeftTop.value);
       m_setCoDriverLeftTopButton.whenPressed(new AdjustArmPosition());
       
//       m_setCoDriverGamepadLeftButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonGameLeft.value);
//       m_setCoDriverGamepadRightButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonGameRight.value);
//       m_setCoDriverGamepadLeftButton.whenPressed(new AdjustArmPosition(-0.01));
//       m_setCoDriverGamepadRightButton.whenPressed(new AdjustArmPosition(0.01));    
       
       
       // Shooter Control
//       m_setCoDriverStartButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonStart.value);
//       m_setCoDriverStartButton.whenPressed(new Shooter(ShooterSubsystem.ShooterValue.kStop.value));

       m_setCoDriverShoulderButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonRightB.value);
       m_setCoDriverRightTriggerButton = new LogitechControllerButton(m_coDriverGamePad, LogitechController.ButtonType.kButtonRightTrigger.value);
     
       m_setCoDriverRightTriggerButton.whenPressed(new Shooter(ShooterSubsystem.ShooterValue.kShoot.value));
       m_setCoDriverRightTriggerButton.whenReleased(new Shooter(ShooterSubsystem.ShooterValue.kStop.value));
       m_setCoDriverShoulderButton.whenPressed(new Shooter(ShooterSubsystem.ShooterValue.kIngest.value));
       m_setCoDriverShoulderButton.whenReleased(new Shooter(ShooterSubsystem.ShooterValue.kStop.value));
       
       
       //
       //
       // Calibration Controller - Calibrating the robot
       //
       //
       m_CalibrationGamePad = new LogitechController(kLogitechCalibrationPort);
       m_setCalibratorLeftShoulderButton = new LogitechControllerButton(m_CalibrationGamePad, LogitechController.ButtonType.kButtonLeftB.value);
       m_setCalibratorLeftTriggerButton = new LogitechControllerButton(m_CalibrationGamePad, LogitechController.ButtonType.kButtonLeftTrigger.value);
       m_setCalibratorRightShoulderButton = new LogitechControllerButton(m_CalibrationGamePad, LogitechController.ButtonType.kButtonRightB.value);
       m_setCalibratorRightTriggerButton = new LogitechControllerButton(m_CalibrationGamePad, LogitechController.ButtonType.kButtonRightTrigger.value);

       m_setCalibratorLeftShoulderButton.whenPressed(new Calibrate(-1));
       m_setCalibratorLeftTriggerButton.whenPressed(new Calibrate(-10));
       m_setCalibratorRightShoulderButton.whenPressed(new Calibrate(1));
       m_setCalibratorRightTriggerButton.whenPressed(new Calibrate(10));

    }      
    
    public LogitechController getDriverController()
    {
        return m_driverGamePad;
    }
    public LogitechController getCoDriverController()
    {
        return m_coDriverGamePad;
    }    
    public LogitechController getCalibrationController()
    {
        return m_CalibrationGamePad;
    }
}

